import numpy as np
def Sensor2ZMP(posList,forceList):
    '''
    更新ZMP点
    :return:
    '''
    total_moment_x = 0
    total_moment_y = 0
    total_pressure = 0
    refPtn = np.mean(np.array(posList),axis=0)
    for i in range(len(posList)):
        sensor_x, sensor_y = posList[i][0],posList[i][1]
        pressure = forceList[i]

        # 计算传感器到参考点的距离
        distance_x = sensor_x - refPtn[0]
        distance_y = sensor_y - refPtn[1]

        # 计算力矩
        moment_x = pressure * distance_x
        moment_y = pressure * distance_y

        total_moment_x += moment_x
        total_moment_y += moment_y
        total_pressure += pressure
    if total_pressure==0:
        return 0,0
        # 计算质心投影位置
    centroid_x = total_moment_x / total_pressure
    centroid_y = total_moment_y / total_pressure
    return centroid_x,centroid_y